#include "Common/Algebra/Matr4.h"

const Matr4 Matr4::IDENTITY = Matr4(
	1.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 1.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 1.0f);

Matr4::Matr4()
{
	for(int i = 0; i < 16; ++i )
		_array[i] = 0.0f;

	_array[0] = 1.0f;
	_array[5] = 1.0f;
	_array[10] = 1.0f;
	_array[15] = 1.0f;
}

Matr4::Matr4(const Matr4& matr)
{
    for(int i = 0; i < 16; ++i )
		_array[i] = matr[i];
}

Matr4::Matr4(float* array)
{
    for(int i = 0; i < 16; ++i )
		_array[i] = array[i];
}

Matr4::Matr4(
	float a11, float a12, float a13, float a14,
	float a21, float a22, float a23, float a24,
	float a31, float a32, float a33, float a34,
	float a41, float a42, float a43, float a44)
{
	_array[0] = a11; _array[4] = a12; _array[8] = a13; _array[12] = a14;
	_array[1] = a21; _array[5] = a22; _array[9] = a23; _array[13] = a24;
	_array[2] = a31; _array[6] = a32; _array[10] = a33; _array[14] = a34;
	_array[3] = a41; _array[7] = a42; _array[11] = a43; _array[15] = a44;
}

Matr4::Matr4(Vec4 col0, Vec4 col1, Vec4 col2, Vec4 col3)
{
	_array[0] = col0.X; _array[4] = _array[0]; _array[8] = _array[4]; _array[12] = _array[8];
	_array[1] = col0.Y; _array[5] = _array[1]; _array[9] = _array[5]; _array[13] = _array[9];
	_array[2] = col0.Z; _array[6] = _array[2]; _array[10] = _array[6]; _array[14] = _array[10];
	_array[3] = col0.W; _array[7] = _array[3]; _array[11] = _array[7]; _array[15] = _array[11];
}

Matr4 Matr4::GetInverted() const
{
	float a[16];

    for(int i = 0; i < 16; ++i )
		a[i] = _array[i];

	float coef = 1.0f/(
						a[0]*a[10]*a[13]*a[7]
						-a[0]*a[10]*a[15]*a[5]
						+a[11]*(
							-a[0]*a[13]*a[6]
							+a[0]*a[14]*a[5]
							-a[12]*a[2]*a[5]
							+a[13]*a[2]*a[4])
						-a[0]*a[14]*a[7]*a[9]
						+a[0]*a[15]*a[6]*a[9]
						+a[1]*(
							-a[10]*a[12]*a[7]
							+a[10]*a[15]*a[4]
							+a[11]*a[12]*a[6]
							-a[11]*a[14]*a[4]
							+a[14]*a[7]*a[8]
							-a[15]*a[6]*a[8])
						+a[10]*a[12]*a[3]*a[5]
						-a[10]*a[13]*a[3]*a[4]
						+a[12]*a[2]*a[7]*a[9]
						-a[12]*a[3]*a[6]*a[9]
						-a[13]*a[2]*a[7]*a[8]
						+a[13]*a[3]*a[6]*a[8]
						+a[14]*a[3]*a[4]*a[9]
						-a[14]*a[3]*a[5]*a[8]
						-a[15]*a[2]*a[4]*a[9]
						+a[15]*a[2]*a[5]*a[8]);

	float array[16];

	array[0] =  a[11]*a[14]*a[5]-a[10]*a[15]*a[5]-a[11]*a[13]*a[6]+a[10]*a[13]*a[7]+a[15]*a[6]*a[9]-a[14]*a[7]*a[9];
	array[1] = -a[1]*a[11]*a[14]+a[3]*a[9]*a[14]+a[1]*a[10]*a[15]+a[11]*a[13]*a[2]-a[10]*a[13]*a[3]-a[15]*a[2]*a[9];
	array[2] = a[15]*a[2]*a[5]-a[14]*a[3]*a[5]-a[1]*a[15]*a[6]+a[13]*a[3]*a[6]+a[1]*a[14]*a[7]-a[13]*a[2]*a[7];
	array[3] = -a[11]*a[2]*a[5]+a[10]*a[3]*a[5]+a[1]*a[11]*a[6]-a[1]*a[10]*a[7]-a[3]*a[6]*a[9]+a[2]*a[7]*a[9];
	array[4] =  -a[11]*a[14]*a[4]+a[10]*a[15]*a[4]+a[11]*a[12]*a[6]-a[10]*a[12]*a[7]-a[15]*a[6]*a[8]+a[14]*a[7]*a[8];
	array[5] =  a[0]*a[11]*a[14]-a[3]*a[8]*a[14]-a[0]*a[10]*a[15]-a[11]*a[12]*a[2]+a[10]*a[12]*a[3]+a[15]*a[2]*a[8];
	array[6] =  -a[15]*a[2]*a[4]+a[14]*a[3]*a[4]+a[0]*a[15]*a[6]-a[12]*a[3]*a[6]-a[0]*a[14]*a[7]+a[12]*a[2]*a[7];
	array[7] =  a[11]*a[2]*a[4]-a[10]*a[3]*a[4]-a[0]*a[11]*a[6]+a[0]*a[10]*a[7]+a[3]*a[6]*a[8]-a[2]*a[7]*a[8];
	array[8] =   a[11]*a[13]*a[4]-a[15]*a[9]*a[4]-a[11]*a[12]*a[5]+a[15]*a[5]*a[8]-a[13]*a[7]*a[8]+a[12]*a[7]*a[9];
	array[9] =  a[1]*a[11]*a[12]-a[3]*a[9]*a[12]-a[0]*a[11]*a[13]-a[1]*a[15]*a[8]+a[13]*a[3]*a[8]+a[0]*a[15]*a[9];
	array[10] =  a[1]*a[15]*a[4]-a[13]*a[3]*a[4]-a[0]*a[15]*a[5]+a[12]*a[3]*a[5]-a[1]*a[12]*a[7]+a[0]*a[13]*a[7];
	array[11] =  -a[1]*a[11]*a[4]+a[3]*a[9]*a[4]+a[0]*a[11]*a[5]-a[3]*a[5]*a[8]+a[1]*a[7]*a[8]-a[0]*a[7]*a[9];
	array[12] =  -a[10]*a[13]*a[4]+a[14]*a[9]*a[4]+a[10]*a[12]*a[5]-a[14]*a[5]*a[8]+a[13]*a[6]*a[8]-a[12]*a[6]*a[9];
	array[13] =  -a[1]*a[10]*a[12]+a[2]*a[9]*a[12]+a[0]*a[10]*a[13]+a[1]*a[14]*a[8]-a[13]*a[2]*a[8]-a[0]*a[14]*a[9];
	array[14] =  -a[1]*a[14]*a[4]+a[13]*a[2]*a[4]+a[0]*a[14]*a[5]-a[12]*a[2]*a[5]+a[1]*a[12]*a[6]-a[0]*a[13]*a[6];
	array[15] =  a[1]*a[10]*a[4]-a[2]*a[9]*a[4]-a[0]*a[10]*a[5]+a[2]*a[5]*a[8]-a[1]*a[6]*a[8]+a[0]*a[6]*a[9];

	for (int i = 0; i < 16; ++i)
		array[i] *= coef;

	return Matr4(array);
}

Matr4 Matr4::operator +=(const Matr4& m ) {
	for (int i = 0; i < 16; ++i)
		_array[i] += m[i];
	return *this;
}

Matr4 Matr4::operator -=(const Matr4& m ) {
	for (int i = 0; i < 16; ++i)
		_array[i] -= m[i];
	return *this;
}

Matr4 Matr4::operator *=(const Matr4& m ) {
	float array[16];

	for (int i = 0; i < 16; ++i)
		array[i] = _array[i];

	_array[0] = array[0] * m[0] + array[4] * m[1] + array[8] * m[2] + array[12] * m[3];
	_array[1] = array[1] * m[0] + array[5] * m[1] + array[9] * m[2] + array[13] * m[3];
	_array[2] = array[2] * m[0] + array[6] * m[1] + array[10] * m[2] + array[14] * m[3];
	_array[3] = array[3] * m[0] + array[7] * m[1] + array[11] * m[2] + array[15] * m[3];

	_array[4] = array[0] * m[4] + array[4] * m[5] + array[8] * m[6] + array[12] * m[7];
	_array[5] = array[1] * m[4] + array[5] * m[5] + array[9] * m[6] + array[13] * m[7];
	_array[6] = array[2] * m[4] + array[6] * m[5] + array[10] * m[6] + array[14] * m[7];
	_array[7] = array[3] * m[4] + array[7] * m[5] + array[11] * m[6] + array[15] * m[7];

	_array[8] = array[0] * m[8] + array[4] * m[9] + array[8] * m[10] + array[12] * m[11];
	_array[9] = array[1] * m[8] + array[5] * m[9] + array[9] * m[10] + array[13] * m[11];
	_array[10] = array[2] * m[8] + array[6] * m[9] + array[10] * m[10] + array[14] * m[11];
	_array[11] = array[3] * m[8] + array[7] * m[9] + array[11] * m[10] + array[15] * m[11];

	_array[12] = array[0] * m[12] + array[4] * m[13] + array[8] * m[14] + array[12] * m[15];
	_array[13] = array[1] * m[12] + array[5] * m[13] + array[9] * m[14] + array[13] * m[15];
	_array[14] = array[2] * m[12] + array[6] * m[13] + array[10] * m[14] + array[14] * m[15];
	_array[15] = array[3] * m[12] + array[7] * m[13] + array[11] * m[14] + array[15] * m[15];
	return *this;
}

Matr4 Matr4::operator *=(float a) {
	for (int i = 0; i < 16; ++i)
		_array[i] *= a;
	return *this;
}

Matr4 Matr4::operator /=(float a) {
	for (int i = 0; i < 16; ++i)
		_array[i] /= a;
	return *this;
}

float Matr4::GetDeterminant( ) const {
	return
		_array[0] * _array[5] * _array[10] +
		_array[2] * _array[4] * _array[9] +
		_array[1] * _array[6] * _array[8] -
		_array[2] * _array[5] * _array[8] -
		_array[1] * _array[4] * _array[10] -
		_array[0] * _array[6] * _array[9];
}

Matr4 Matr4::GetTransposed( ) const {
	return Matr4(
		_array[0], _array[4], _array[8], _array[12],
		_array[1], _array[5], _array[9], _array[13],
		_array[2], _array[6], _array[10], _array[14],
		_array[3], _array[7], _array[11], _array[15] );
}
